' EtherCAT总线控制 
Dim connectFlag
connectFlag=0
Dim num,IO_Num
IO_Num=0
Dim g_ecatmode
g_ecatmode = 0 '0:csp 2:cst
Dim a
a=0
' While 1
	' a=a+1
	' Call TASK

' Wend 

' sub TASK()
SLOT_SCAN(0) '总线扫描 
If Return Then 
	? "设备个数：",NODE_COUNT(0)	'挂载设备个数
	num=NODE_COUNT(0)-IO_Num
	For i=0 To num-1
		SDO_WRITE (0,i,$1C12,0,5,0)
		SDO_WRITE (0,i,$1C13,0,5,0)
		SDO_WRITE (0,i,$1C12,1,3,$1600)
		SDO_WRITE (0,i,$1C12,0,5,1)      
		SDO_WRITE (0,i,$1C13,1,3,$1A00)
		SDO_WRITE (0,i,$1C13,0,5,1) 
		?i,NODE_AXIS_COUNT (0, i) '挂载电机个数
	Next i
	? "SDO 写入完成"
	wa 10
	For i=0 To num-1
		AXIS_ADDRESS(i)=(0<<16) + i	+ 1		'映射轴号
		If g_ecatmode=2 Then 
			' SDO_WRITE(0,0,$6060,0,6,10)'转矩模式
			' SDO_WRITE(0,0,$2032,0,6,50)'转矩速度
			' SDO_WRITE(0,0,$6071,0,3,150)'目标转矩
			' ATYPE(i)=67 '轴类型67转矩模式
			' DRIVE_PROFILE(i) =137	'驱动器PDO设置
		ElseIf g_ecatmode=0 Then 
			ATYPE(i)=65 '轴类型66，速度模式 
			
			DRIVE_PROFILE(i) =100	'驱动器PDO设置
			
		' DRIVE_PROFILE(i) =137	'驱动器PDO设置
		' DRIVE_CONTROLWORD(i)=6
		' DRIVE_CONFIG(0,1) = $10
		EndIf 
	Next i 
	? "PDO设置完成"

	For i=num To 7+num	'对应EMC108R控制器
		AXIS_ADDRESS(i)=(-1<<16) + i-num	'映射本地轴号到总线轴后面
	Next i 
	
	SLOT_START(0) '开启总线 
	If Return Then
        ?"总线开启成功"
    Else
        ?"总线开启失败"
    End If
	WA(1000)

	For i=0 To num-1
		DRIVE_CONTROLWORD(i)=128'Fault reset 
		wa 100
		
		? i,HEX(drive_status(i)),DRIVE_CONTROLWORD(i)'128伺服错误清除
	Next i
	delay 1000
	For i=0 To num-1 
		DRIVE_CONTROLWORD(i)=6'Shutdown
		wa 100
		? i,HEX(drive_status(i)),DRIVE_CONTROLWORD(i)'伺服关闭
	Next i
	delay 1000
	For i=0 To num-1 
		DRIVE_CONTROLWORD(i)=7'Shutdown
		wa 100
		? i,HEX(drive_status(i)),DRIVE_CONTROLWORD(i)'1231主回路电源关闭
	Next i
	delay 1000
	
	For i=0 To num-1 
		DRIVE_CONTROLWORD(i)=15   'Switch On + Enable operation
		wa 100
		? i,HEX(drive_status(i)),DRIVE_CONTROLWORD(i)'15伺服开启
	Next i
	delay 1000
	?"轴使能准备"
		WDOG =1
		
	'连接成功标志
	connectFlag=1
Else
	Print "总线扫描失败"
	GoTo TASK
EndIf

' End Sub 

